/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "FleetFormationGUI.hpp"
CFleetFormationGUI::CFleetFormationGUI():
  CSync(1,1),
  MaCI::Position::CPositionClientCallback(),
  iGimi(NULL),
  iSDLScreen(NULL),
  iWidth(800),
  iHeigth(600),
  iInitialized(false),
  iRunGUI(false),
  iGridminX(0),
  iGridmaxX(iWidth-100),
  iGridminY(100),
  iGridmaxY(iHeigth),
  iServiceList(),
  iRobotMap(),
  iMasterRobotIter(iRobotMap.end()),
  iSelectedRobotIterator(iRobotMap.end()),
  iGUIButtons(),
  iPixelsToMeter(100),
  iFleetController(),
  iMasterPC(NULL),
  iMasterSC(NULL),
  iSlavePC(),
  iSlaveSC()
  
{
  CreateButtons();
}
//*****************************************************************************
//*****************************************************************************

CFleetFormationGUI::~CFleetFormationGUI()
{
  DeleteMachines();

}
//*****************************************************************************
//*****************************************************************************

bool CFleetFormationGUI::Initialize(gimi::GIMI *aGimiPtr,int aGimboID, int aReportID)
{
  iGimi = aGimiPtr;
  iInitialized = true;
  // Start the Gimbo component core.
  CComponent::Start(aGimiPtr, aGimboID, aReportID);
  return true;
}
//*****************************************************************************
//*****************************************************************************

void CFleetFormationGUI::Deinitialize(void)
{



}
//*****************************************************************************
//*****************************************************************************
void CFleetFormationGUI::StartGUI()
{

  InitializeSDL();

  RunGUI();  
}
//*****************************************************************************

bool CFleetFormationGUI::InitializeSDL()
{


  /////////////////////////////////////////////////////////////////////////////
  // Initialize SDL
  dPrint(1,"Initializing SDL VIDEO");
  if(SDL_Init(SDL_INIT_VIDEO) == -1){
    dPrint(1,"Failed to initialize SDL Video: %s\n", SDL_GetError());
    exit(1);
  }
  
  // Register cleanup function for SDL
  atexit(SDL_Quit);
  
  dPrint(1,"Setting video mode to %d x %d",iWidth,iHeigth);
  // Initialize SDL-Video in mode WW*WH, 32bpp, & some flags.
  if ((iSDLScreen = SDL_SetVideoMode(iWidth,iHeigth, 32,
				 SDL_HWSURFACE|SDL_DOUBLEBUF|SDL_RESIZABLE)) == NULL) {
    dPrint(1, "Unable to set video mode for SDL Context: %s\n", SDL_GetError());
    return false;
  }
  
  // Clear the screen
  SDL_FillRect(iSDLScreen, NULL, 0);

  // Set imporant info :)
  SDL_WM_SetCaption("Fleet Formation application","FleetFormationGUI");

  return true;
}
//*****************************************************************************

void CFleetFormationGUI::RunGUI()
{

  iRunGUI = true;
  bool isRobotAttached = false;
  std::map<std::string,SRobotIcon *>::iterator attachedRobotIterator;
  
  while(iRunGUI){
    SDL_Event event;
    
    // Wait for incoming event
    while (SDL_PollEvent(&event)) {
      //dPrint(1,"event...");
      // Switch based on event type.
      switch (event.type) {
        
        // QUIT ('Window close' clicked)
      case SDL_QUIT: {
        dPrint(1,"Got SDL_QUIT");
        iRunGUI = false;
        ownSleep_ms(1000);
        //raise(SIGKILL);
        break;
      }
        // Any key down
      case SDL_KEYDOWN: {
        // Switch based on the key that was pressed
        switch (event.key.keysym.sym) {  
          case SDLK_s:{

          SetRobotsToFleetFormationController();
          break;
          }

        case SDLK_DELETE:{
          if(attachedRobotIterator != iRobotMap.end()){
            //             iFleetController->RemoveSlaveMachine();

          }
          break;
        }
        default:
          break;
        
         
        
          break;
        }

      
      }
      case SDL_MOUSEBUTTONDOWN:{
        //        dPrint(1,"Mouse pressed %d %d",event.button.x,event.button.y);
        std::map<std::string,SRobotIcon *>::iterator iter = iRobotMap.begin();
        SDL_FillRect(iSDLScreen, NULL, 0);
        //dPrint(1,"size of map %d",iRobotMap.size());
        if(event.button.button == SDL_BUTTON_LEFT &&
           event.button.x <= iGUIButtons[0]->xMax &&
           event.button.x >= iGUIButtons[0]->xMin &&
           event.button.y >= iGUIButtons[0]->yMin &&
           event.button.y <= iGUIButtons[0]->yMax){

          SetRobotsToFleetFormationController();

        }

        while(iter != iRobotMap.end()){
          if(iter->second->IsInsideRobot(event.button.x,event.button.y)){
            
            if(event.button.button == SDL_BUTTON_LEFT){
              isRobotAttached = true;          
              attachedRobotIterator = iter;
              iSelectedRobotIterator = iter;
            }else if(event.button.button == SDL_BUTTON_RIGHT){
              if(iter != iMasterRobotIter){
                iMasterRobotIter =iter;

              }else{
                iter = iRobotMap.end();
              }
              
            }


            break;
          }
          iter++;
        }
        break;
      }

      case SDL_MOUSEBUTTONUP:{
        isRobotAttached = false;
      }

      case SDL_MOUSEMOTION:{
        //dPrint(1,"mouse motion %d %d",event.motion.x,event.motion.y);

        if(isRobotAttached){
          attachedRobotIterator->second->SetPosition(event.motion.x,event.motion.y);

        }
        break;


      }

      }


      DrawScreen();
      
    }

    ownSleep_ms(50);
   
  }


}
//*****************************************************************************

void CFleetFormationGUI::StopGUI()
{
  iRunGUI = false;

}

//*****************************************************************************
//*****************************************************************************
void CFleetFormationGUI::OnServiceAssignment(const Gimbo::TServiceAssignmentList &aServiceList)
{
  DeleteMachines();


  dPrint(1,"ON service assignment. size %d, size o fthe robot map %d",aServiceList.size(),iRobotMap.size());
  Lock();
  
  iServiceList= aServiceList;
  Gimbo::TServiceAssignmentList::size_type index = 0;
  MaCI::Position::CPositionClient *posClient;
  MaCI::SpeedCtrl::CSpeedCtrlClient *speedClient;
  while(index < aServiceList.size()){
    //dPrint(1," %s",aServiceList[index].MaCISL.c_str());

    MaCI::MaCICtrl::SMaCISL maciSL(aServiceList[index].MaCISL);

    //    dPrint(1,"Checking if the machine exists. Size of the map %d",iRobotMap.size());
    //IF THE MACHINE DOES NOT EXIST
    if(iRobotMap.find(maciSL.group) == iRobotMap.end()){
      //dPrint(1,"Machine does not exist. size of the map %d",iRobotMap.size());
      int poseX = iRobotMap.size()* 50 +10;

      int poseY =20;

      if(poseX > (int)iWidth-50){
        poseX -= (iWidth -50);
        poseY = 50;
      }
      
      
      if(maciSL.interface == "MaCI_Position"){
        dPrint(1,"Creating robot to %d %d. Size of the map is %d",poseX,poseY,iRobotMap.size());
        SRobotIcon *icon = new SRobotIcon(poseX,poseY);
        iRobotMap[maciSL.group] = icon;
        dPrint(1,"Creating new position client");
        posClient = new MaCI::Position::CPositionClient(iGimi,0);
        dPrint(1,"Setting default target");
        posClient->SetDefaultTarget(aServiceList[index].targetID);
        if(posClient->Open() == MaCI::KMaCIOK){
          dPrint(1,"PositionClientOpened");
          //posClient->SetPositionClientCallback(this);
          icon->iPositionClient = posClient;
          iRobotMap[maciSL.group] = icon;
        }else{
          delete(icon);

        }
      }else if(maciSL.interface =="MaCI_SpeedCtrl"){
          dPrint(1,"Creating robot to %d %d. Size of the map is %d",poseX,poseY,iRobotMap.size());
        SRobotIcon *icon = new SRobotIcon(poseX,poseY);
        iRobotMap[maciSL.group] = icon;
        speedClient = new MaCI::SpeedCtrl::CSpeedCtrlClient(iGimi,0);
        speedClient->SetDefaultTarget(aServiceList[index].targetID);
        if(speedClient->Open() == MaCI::KMaCIOK){
          icon->iSpeedCtrlClient = speedClient;
          iRobotMap[maciSL.group] = icon;
        }else{
          delete(icon);
          dPrint(1,"Failed to open speedctrl client");
        }

      }


      //If it exists, check if it has coordinatedrive or position interfaces
    }else{
      //dPrint(1,"Machine already exist.Size of the map %d",iRobotMap.size());
      if(maciSL.interface.compare(std::string("MaCI_Position")) == 0){
        posClient = new MaCI::Position::CPositionClient(iGimi,0);
        posClient->SetDefaultTarget(aServiceList[index].targetID);
        if(posClient->Open() == MaCI::KMaCIOK){
          //          posClient->SetPositionClientCallback(this);
          iRobotMap[maciSL.group]->iPositionClient = posClient;
        }else{
          dPrint(1,"Failed to open position client. ");
        }
      }else if(maciSL.interface.compare(std::string("MaCI_SpeedCtrl")) == 0){
        speedClient = new MaCI::SpeedCtrl::CSpeedCtrlClient(iGimi,0);
        speedClient->SetDefaultTarget(aServiceList[index].targetID);
        if(speedClient->Open() == MaCI::KMaCIOK){
          iRobotMap[maciSL.group]->iSpeedCtrlClient = speedClient;
        }else{
          
          dPrint(1,"Failed to open speedctrl client");
        }
      }
    }



    
    index++;
  }
  Unlock();
  
} 

//*****************************************************************************

void CFleetFormationGUI::DrawScreen()
{
  //dPrint(1,"Draw screen called");
  /////////DRAW ROBOTS
  float x1,y1,x2,y2,x3,y3;

  Lock();

  std::map<std::string,SRobotIcon *>::iterator iter = iRobotMap.begin();
  SDL_FillRect(iSDLScreen, NULL, SDL_MapRGB( iSDLScreen->format, 155, 155, 155 ));
  //  dPrint(1,"size of map %d",iRobotMap.size());

  DrawGridAndScale();
  DrawButtons();

  int masterX=-1;
  int masterY = -1;


  if(iMasterRobotIter != iRobotMap.end()){
    masterX = iMasterRobotIter->second->positionX;
    masterY = iMasterRobotIter->second->positionY;
  }
  //Draw robots
  while(iter != iRobotMap.end()){

    
    iter->second->GetTriangleCorners(x1,y1,x2,y2,x3,y3);
    //   dPrint(1,"Drawing to; %f %f;%f %f;%f %f; ",x1,y1,x2,y2,x3,y3);
    float alfa = 100;
    int red = 200;
    int green = 200;
    int blue = 0;
    if(iter == iSelectedRobotIterator){
      alfa = 255;
      red = 255;
      green = 255;
    }
    if(iter == iMasterRobotIter){
      filledTrigonRGBA(iSDLScreen, x1,y1,
                       x2,y2,
                       x3,y3,
                       red,green,blue, alfa);
                       
    }else{
      lineRGBA(iSDLScreen,
               x1,y1,
               x2,y2,
               red,green,blue, alfa);
      
      lineRGBA(iSDLScreen,
               x2,y2,
               x3,y3,
               red,green,blue, alfa);
      
      lineRGBA(iSDLScreen,
               x3,y3,
               x1,y1,
               red,green,blue, alfa);

      

      if(masterX != -1 && masterY !=-1 && iter->second->positionY > 100 &&  iter->second->positionX < iWidth-100){
        lineRGBA(iSDLScreen,
                 masterX,masterY,
                 iter->second->positionX,iter->second->positionY,
                 0,125,0,100);
      }
    }

    int textlength = iter->first.size();
    stringRGBA(iSDLScreen,
               x1-textlength*4,y1-10,
               iter->first.c_str(),
               0,0,255,150);
    ++iter;
  }
  Unlock();



  // Flip the double buffered screen. (Display the newly rendered screen)
  SDL_Flip(iSDLScreen);



}
//*****************************************************************************

void CFleetFormationGUI::DrawGridAndScale(){

  //"Parkki"
  lineRGBA(iSDLScreen,
           0,100,
           iWidth,100,
           255,0,0,255);
  
  //Nappula frame

  lineRGBA(iSDLScreen,
           iWidth-100,100,
           iWidth-100,iHeigth,
           255,0,0,255);
  //Draw Grid
  
  for(unsigned int i = iPixelsToMeter; i<iHeigth; i=i+iPixelsToMeter){
    
    lineRGBA(iSDLScreen,
             0,i,
             iWidth-100,i,
             128,128,128,128);
  }
  
  for(unsigned int j = iPixelsToMeter; j<iWidth; j=j+iPixelsToMeter){
    lineRGBA(iSDLScreen,
             j,100,
             j,iHeigth,
             128,128,128,128);
             
  }

 //Draw the scale

  lineRGBA(iSDLScreen,
           100,iHeigth - 50,
           200,iHeigth - 50,
           0,255,0,100);

  lineRGBA(iSDLScreen,
           100,iHeigth - 50,
           100,iHeigth - 60,
           0,255,0,100);
  lineRGBA(iSDLScreen,
           100+iPixelsToMeter,iHeigth - 50,
           100+iPixelsToMeter,iHeigth - 60,
           0,255,0,100);


  std::string onemeter = "1 m";

  stringRGBA(iSDLScreen,
             140,iHeigth- 40,
             onemeter.c_str(),
             0,0,255,150);
}
//*****************************************************************************
void CFleetFormationGUI::DrawButtons()
{
  for(unsigned int i = 0; i < iGUIButtons.size(); i++){
    SDL_Rect r;
    //dPrint(1,"Button %d %d %d %d",iGUIButtons[i]->xMin,iGUIButtons[i]->yMin,iGUIButtons[i]->xMax, iGUIButtons[i]->yMax);
    r.x = iGUIButtons[i]->xMin;
    r.y = iGUIButtons[i]->yMin;
    r.w = iGUIButtons[i]->xMax -iGUIButtons[i]->xMin;
    r.h = iGUIButtons[i]->yMax -iGUIButtons[i]->yMin;
    //dPrint(1,"Drawing button %d %d %d %d ",r.x,r.y,r.w,r.h);
    SDL_FillRect(iSDLScreen,&r,0);
    stringRGBA(iSDLScreen,
               iGUIButtons[i]->xMin + 3,(iGUIButtons[i]->yMax +iGUIButtons[i]->yMin)/2,
               iGUIButtons[i]->text.c_str(),
               255,255,255,150);
    

  }
  

}

//*****************************************************************************


void CFleetFormationGUI::SetRobotsToFleetFormationController()
{
  //Remove the old formation
  iFleetController.RemoveMasterMachine();
  iFleetController.RemoveAllSlaves();

  //Then add the master
  if(iMasterRobotIter == iRobotMap.end()){
    dPrint(1,"No master robot set. Can't go to formation");
    return;
  }

  int masterX=iMasterRobotIter->second->positionX;
  int masterY =iMasterRobotIter->second->positionY;
  
  if(masterX < iGridminX ||
     masterX > iGridmaxX ||
     masterY < iGridminY ||
     masterY > iGridmaxY){
    dPrint(1,"Master not set or not in the grid. Can't go to formation");
    return;
  }

  if(iMasterRobotIter->second->iPositionClient && iMasterRobotIter->second->iSpeedCtrlClient){
    iFleetController.SetMasterMachine(iMasterRobotIter->second->iPositionClient,
                                      iMasterRobotIter->second->iSpeedCtrlClient,
                                      iMasterRobotIter->first);  
  }else{
    dPrint(1,"Failed to find master position and/or SpeedCtrl services");
    return ;
  }



  //And then the slaves
  dPrint(1,"Searching the slaves. Number of robots %d",iRobotMap.size());
  std::map<std::string,SRobotIcon *>::iterator iter = iRobotMap.begin();
  while(iter != iRobotMap.end()){
    if(iter->second->positionX < iGridminX ||
       iter->second->positionX > iGridmaxX ||
       iter->second->positionY < iGridminY ||
       iter->second->positionY > iGridmaxY){
      dPrint(1,"Robot not in grid. skipping it (%d,%d) GRID x(%d,%d) y(%d,%d)", iter->second->positionX,iter->second->positionY,iGridminX,iGridmaxX,iGridminY,iGridmaxY);
      iter++;
      continue;

    }

    if(iter == iMasterRobotIter){
      iter++;
      continue;
    }


    if(iter->second->iSpeedCtrlClient != NULL &&iter->second->iPositionClient !=NULL){
      MaCI::Position::TPose2D slavePose;
      float relativeY = (float)(masterX - iter->second->positionX)/iPixelsToMeter;
      
      //Because X is flipped
      float relativeX = (float)(masterY - iter->second->positionY)/iPixelsToMeter;
      ///Slave's relative positions
      slavePose.x = relativeX;
      slavePose.y = relativeY;
      slavePose.a = 0.0;
      iFleetController.AddSlaveMachine(iter->second->iPositionClient,
                                       iter->second->iSpeedCtrlClient,
                                       slavePose);
    }

    iter++;    
  }
  
  
  
  dPrint(1,"Go To Formation!");
  iFleetController.GoToFormation();
  iFleetController.StartToFollow();
  


}
//*****************************************************************************

void CFleetFormationGUI::CreateButtons(){

  SFleetGUIButton *button = new SFleetGUIButton(iWidth-90, 110,
                                                iWidth-10, 190,
                                                std::string("Set to formation"));
  iGUIButtons.push_back(button);
                                                

}
//*****************************************************************************

void CFleetFormationGUI::DeleteMachines(){

  iFleetController.RemoveMasterMachine();
  iFleetController.RemoveAllSlaves();


  iSlavePC.clear();
  iSlaveSC.clear();
}
//*****************************************************************************
void CFleetFormationGUI::OnPositionDataEvent(const MaCI::Position::CPositionData &aData, 
                                             const unsigned int aDataSequence,
                                             MaCI::Position::CPositionClient &aPositionClient,
                                             void * aUserPtr){



}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
